package com.google.android.apps.skipbot;

/**
 * High level class for the Skipper robot that provides user understandable
 * controls.
 */
public class SkipperRobot extends Robot implements SkipperRobotController {

  private static final int CALIBRATION_ANGLE = 90;
  private RobotMovementSequence sitSequence = null;
  private RobotMovementSequence standSequence = null;
  private GenericIKGaitSequence walkSequence = null;

  private final SkipperLeg[] legs;
  private final SkipperRobotConfigInterface skipperRobotConfig;

  public SkipperRobot(RobotController robotController, SkipperRobotConfigInterface robotConfig) {
    super(robotController, robotConfig);
    this.legs = SkipperLeg.createLegArray(this, robotConfig, new SkipperIKCalculator(robotConfig));
    this.skipperRobotConfig = robotConfig;
  }

  @Override
  public void moveForward() {
    move(0, GaitType.TRIPOD);
  }

  @Override
  public void stand() {
    if (standSequence == null) {
      this.standSequence = new SkipperStandSequence(skipperRobotConfig, this);
    }
    executeMovementSequences(standSequence);
  }

  @Override
  public void sit() {
    if (sitSequence == null) {
      this.sitSequence = new SkipperSitSequence(skipperRobotConfig, this);
    }
    executeMovementSequences(sitSequence);
  }

  @Override
  public void stop() {
    super.stop();
    this.walkSequence = null;
  }

  @Override
  public int getCalibrationAngle(int servoIndex) {
    return CALIBRATION_ANGLE;
  }

  /**
   * Move in the direction specified in radians. This method can be called while
   * the robot is moving and it will just change direction or gait.
   *
   * @param angle in radians measured clockwise where 0 is forward
   * @param gaitType is the type of gait the robot will walk with
   */
  public void move(double angle, GaitType gaitType) {
    IKSequenceConfig config;
    switch (gaitType) {
      case TRIPOD:
        if (skipperRobotConfig.getScaleFactor() < 1.0) {
          config = IKSequenceConfig.TRIPOD_GAIT_SMALL;
        } else {
          config = IKSequenceConfig.TRIPOD_GAIT;
        }
        break;
      case RIPPLE:
        if (skipperRobotConfig.getScaleFactor() < 1.0) {
          config = IKSequenceConfig.RIPPLE_GAIT_SMALL;
        } else {
          config = IKSequenceConfig.RIPPLE_GAIT;
        }
        break;
      case STAGGERED:
        if (skipperRobotConfig.getScaleFactor() < 1.0) {
          config = IKSequenceConfig.STAGGER_GAIT_SMALL;
        } else {
          config = IKSequenceConfig.STAGGER_GAIT;
        }
        break;
      default:
        if (skipperRobotConfig.getScaleFactor() < 1.0) {
          config = IKSequenceConfig.TRIPOD_GAIT_SMALL;
        } else {
          config = IKSequenceConfig.TRIPOD_GAIT;
        }
    }

    // Check if we are currently moving with the same gait and if so just change
    // the angle.
    if (this.walkSequence != null) {
      IKSequenceConfig currentConfig = (IKSequenceConfig) this.walkSequence.getSequenceConfig();
      if (currentConfig == config) {
        this.walkSequence.setAngle(angle);
        return;
      }
    }

    this.walkSequence = new GenericIKGaitSequence(skipperRobotConfig, this, config, angle);
    executeMovementSequences(walkSequence);
  }

  @Override
  public void moveLeg(int legIndex, double[] position) {
    getLeg(legIndex).moveTo(position);
  }

  @Override
  public void moveAllLegs(double[][] position) {
    int[] angles = new int[getNumServos()];
    for (int i = 0; i < skipperRobotConfig.getNumLegs(); ++i) {
      int[] legAngles = getLeg(i).getAngles(position[i]);
      angles[skipperRobotConfig.getHipServoIndex(i)] = legAngles[0];
      angles[skipperRobotConfig.getShoulderServoIndex(i)] = legAngles[1];
      angles[skipperRobotConfig.getElbowServoIndex(i)] = legAngles[2];
    }
    moveAllServos(angles);
  }

  private SkipperLeg getLeg(int legIndex) {
    return legs[legIndex];
  }
}
